//
// Created by fins on 24-9-28.
//
#include "lib/cloud_filter.hpp"
#include "lib/cube.hpp"
#include <chrono>

CloudFilter::CloudFilter() {
}

void CloudFilter::loadParams(const std::string& yaml_file) {
    YAML::Node config = YAML::LoadFile(yaml_file);
    transform_to_robot_frame = config["transform_to_robot_frame"].as<std::string>();

    box_min_.x() = config["box_size"]["min"]["x"].as<float>();
    box_min_.y() = config["box_size"]["min"]["y"].as<float>();
    box_min_.z() = config["box_size"]["min"]["z"].as<float>();
    box_max_.x() = config["box_size"]["max"]["x"].as<float>();
    box_max_.y() = config["box_size"]["max"]["y"].as<float>();
    box_max_.z() = config["box_size"]["max"]["z"].as<float>();

    // 初始化立方体cube
    cube_ = Cube(Eigen::Vector3f(box_min_.x(), box_min_.y(), box_min_.z()),
                 Eigen::Vector3f(box_max_.x(), box_max_.y(), box_max_.z()));
    printf("box_min: %f %f %f\n", box_min_.x(), box_min_.y(), box_min_.z());
    printf("box_max: %f %f %f\n", box_max_.x(), box_max_.y(), box_max_.z());
}

void CloudFilter::filter(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr& output_cloud) {
    if (transform_to_robot_frame=="true") {
        pcl::CropBox<pcl::PointXYZ> box_filter;
        box_filter.setInputCloud(input_cloud);

        Eigen::Vector4f min_point(box_min_.x(), box_min_.y(), box_min_.z(), 1.0);
        Eigen::Vector4f max_point(box_max_.x(), box_max_.y(), box_max_.z(), 1.0);

        box_filter.setMin(min_point.head<4>());
        box_filter.setMax(max_point.head<4>());
        box_filter.setNegative(true);

        box_filter.filter(*output_cloud);
    } else {
        auto start_time = std::chrono::high_resolution_clock::now();
        for (const auto& point : input_cloud->points) {
            if (not cube_.contains(Eigen::Vector3f(point.x, point.y, point.z))) {
                output_cloud->points.push_back(point);
            }
        }
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        std::cout << "Cube cloud processing took " << duration << " milliseconds" << std::endl;
}}

void CloudFilter::set_transform_r2l(const geometry_msgs::msg::TransformStamped& transform) {
    transform_r2l = transform;
    cube_.transform(transform);
}

CloudFilter_Custom::CloudFilter_Custom() {
}

void CloudFilter_Custom::loadParams(const std::string& yaml_file) {
    YAML::Node config = YAML::LoadFile(yaml_file);
    transform_to_robot_frame = config["transform_to_robot_frame"].as<std::string>();

    box_min_.x() = config["box_size"]["min"]["x"].as<float>();
    box_min_.y() = config["box_size"]["min"]["y"].as<float>();
    box_min_.z() = config["box_size"]["min"]["z"].as<float>();
    box_max_.x() = config["box_size"]["max"]["x"].as<float>();
    box_max_.y() = config["box_size"]["max"]["y"].as<float>();
    box_max_.z() = config["box_size"]["max"]["z"].as<float>();

    cube_ = Cube(Eigen::Vector3f(box_min_.x(), box_min_.y(), box_min_.z()),
                 Eigen::Vector3f(box_max_.x(), box_max_.y(), box_max_.z()));
    printf("box_min: %f %f %f\n", box_min_.x(), box_min_.y(), box_min_.z());
    printf("box_max: %f %f %f\n", box_max_.x(), box_max_.y(), box_max_.z());
}

void CloudFilter_Custom::filter(const CustomMsgPtr& input_cloud, CustomMsgPtr& output_cloud) {

    output_cloud->points.clear();
    output_cloud->header = input_cloud->header;  // 复制消息头
    output_cloud->timebase = input_cloud->timebase; // 复制时间基准
    output_cloud->point_num = 0; // 初始化点云数量
    output_cloud->lidar_id = input_cloud->lidar_id; // 复制 Lidar ID
    std::fill(std::begin(output_cloud->rsvd), std::end(output_cloud->rsvd), 0); // 初始化保留字段
    if (transform_to_robot_frame=="true"){
    for (const auto& point : input_cloud->points) { // 使用 -> 访问成员
        // printf("point: %f %f %f\n", point.x, point.y, point.z);
        if (point.x < box_min_.x()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        } else if (point.x > box_max_.x()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        } else if (point.y < box_min_.y()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        } else if (point.y > box_max_.y()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        } else if (point.z < box_min_.z()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        } else if (point.z > box_max_.z()) {
            output_cloud->points.push_back(point);
            output_cloud->point_num++;
        }
    }}else {
        auto start_time = std::chrono::high_resolution_clock::now();
        for (const auto& point : input_cloud->points) {
            if (not cube_.contains(Eigen::Vector3f(point.x, point.y, point.z))) {
                output_cloud->points.push_back(point);
            }
        }
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        std::cout << "Cube cloud processing took " << duration << " milliseconds" << std::endl;
    }
}

void CloudFilter_Custom::set_transform_r2l(const geometry_msgs::msg::TransformStamped& transform) {
    transform_r2l = transform;
    cube_.transform(transform);
}

// fromRosMsg 函数实现
CustomMsg fromRosMsg(const livox_ros_driver2::msg::CustomMsg_<std::allocator<void>>& ros_msg) {
    CustomMsg custom_msg;
    custom_msg.header = ros_msg.header;
    custom_msg.timebase = ros_msg.timebase;
    custom_msg.point_num = ros_msg.point_num;
    custom_msg.lidar_id = ros_msg.lidar_id;
    custom_msg.rsvd = ros_msg.rsvd;


    custom_msg.points.resize(ros_msg.points.size());
    for (size_t i = 0; i < ros_msg.points.size(); ++i) {
        const auto& ros_point = ros_msg.points[i];  // 获取当前 ROS 点
        CustomPoint& custom_point = custom_msg.points[i];  // 获取对应的 CustomPoint


        custom_point.offset_time = ros_point.offset_time;
        custom_point.x = ros_point.x;
        custom_point.y = ros_point.y;
        custom_point.z = ros_point.z;
        custom_point.reflectivity = ros_point.reflectivity;
        custom_point.tag = ros_point.tag;
        custom_point.line = ros_point.line;
    }

    return custom_msg;  // 返回转换后的 CustomMsg
}

// toRosMsg 函数实现
livox_ros_driver2::msg::CustomMsg_<std::allocator<void>> toRosMsg(const CustomMsg& custom_msg) {
    livox_ros_driver2::msg::CustomMsg_<std::allocator<void>> ros_msg;  // 创建一个 ROS 消息实例
    ros_msg.header = custom_msg.header;  // 转换标准头部
    ros_msg.timebase = custom_msg.timebase;
    ros_msg.point_num = custom_msg.point_num;
    ros_msg.lidar_id = custom_msg.lidar_id;
    ros_msg.rsvd = custom_msg.rsvd;

    // Resize points vector and populate
    ros_msg.points.resize(custom_msg.points.size());
    for (size_t i = 0; i < custom_msg.points.size(); ++i) {
        const CustomPoint& custom_point = custom_msg.points[i];  // 获取当前 CustomPoint
        auto& ros_point = ros_msg.points[i];  // 获取对应的 ROS 点

        // 逐个字段赋值
        ros_point.offset_time = custom_point.offset_time;
        ros_point.x = custom_point.x;
        ros_point.y = custom_point.y;
        ros_point.z = custom_point.z;
        ros_point.reflectivity = custom_point.reflectivity;
        ros_point.tag = custom_point.tag;
        ros_point.line = custom_point.line;
    }
    return ros_msg;  // 返回转换后的 ROS 消息
}